/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#ifndef _POSITION_H_
#define _POSITION_H_

#include <stdint.h>
#include "attitude.h"
#include "zino.h"

#ifdef __cplusplus
extern "C"
{
#endif
#define ALT_SAVED_FLAG 0xAF
#define POS_SAVED_FLAG 0xBF
    enum
    {
        ALT_POS = 0,
        ALT_VEL,
        ALT_COUNT
    };
    enum
    {
        HOR_POS = 0, // horizontal position
        HOR_VEL,     // horizontal velocity
        HOR_COUNT
    };
    enum
    {
        POS_VX = 0, // horizontal position
        POS_VY,     // horizontal velocity
        POS_PX,
        POS_PY,
        POS_PID_COUNT
    };
    struct altHoldPidConfig
    {
        uint8_t flag;
        uint8_t throttleMax;
        pidf_t pid[ALT_COUNT]; // pos, vel
    };
    struct posHoldPidConfig
    {
        uint8_t flag;
        pidf_t pid[POS_PID_COUNT]; // vx,vy,px,py
    };
    ZINO_CONFIG_DECLARE(struct altHoldPidConfig, altHoldPidConfig);
    ZINO_CONFIG_DECLARE(struct posHoldPidConfig, posHoldPidConfig);

    void position_z_pid_reload(void);
    void position_z_pid_init(void);
    void position_z_pid_save(void);
    void position_z_pid_reset_default(void);
    void position_xy_pid_reload(void);
    void position_xy_pid_init(void);
    void position_xy_pid_save(void);
    void position_xy_pid_reset_default(void);

    void position_z_control(float dt);
    void position_xy_control(float dt);

    bool is_motor_warmup_ready(void);

#ifdef __cplusplus
}
#endif
#endif
